# 智能送药小车 - 极简红线检测版本
# 功能：只检测红线存在与否，有红线发送RED,1，无红线发送NONE,0

import sensor
import time
from pyb import UART

# 初始化UART用于与STM32通信
uart = UART(3, 115200)

# 红色阈值 (需要根据实际红色胶带调整)
# LAB颜色空间: (L_min, L_max, A_min, A_max, B_min, B_max)
# 这个阈值可能需要根据您的红色胶带进行调整
red_threshold = (10, 70, 15, 70, 10, 60)

# 摄像头初始化
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)  # 320x240分辨率
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)  # 关闭自动增益
sensor.set_auto_whitebal(False)  # 关闭自动白平衡

# ROI区域设置 - 只检测画面下半部分
LINE_ROI = (0, 120, 320, 120)    # 检测区域（画面下半部分）

# 发送计数器，用于控制发送频率
send_counter = 0

def detect_red_line():
    """简单检测红色线段是否存在"""
    global send_counter
    
    img = sensor.snapshot()
    
    # 在ROI区域寻找红色区域
    blobs = img.find_blobs([red_threshold], 
                          roi=LINE_ROI,
                          pixels_threshold=20,   # 降低像素阈值
                          area_threshold=20,     # 降低面积阈值
                          merge=True)
    
    # 绘制检测区域
    img.draw_rectangle(LINE_ROI, color=(255, 255, 0))
    
    has_red = False
    
    if blobs:
        # 找到任何红色区域就认为有红线
        largest_blob = max(blobs, key=lambda b: b.pixels())
        
        # 绘制识别结果
        img.draw_rectangle(largest_blob.rect(), color=(0, 255, 0))
        img.draw_cross(largest_blob.cx(), largest_blob.cy(), color=(255, 0, 0))
        
        has_red = True
    
    # 控制发送频率，每3次检测发送一次结果
    send_counter += 1
    if send_counter >= 3:
        send_counter = 0
        if has_red:
            uart.write("RED,1\r\n")
            print("发送: RED,1")
        else:
            uart.write("NONE,0\r\n")
            print("发送: NONE,0")
    
    return has_red

# 主循环
while True:
    # 检测红线
    has_red_line = detect_red_line()
    
    # 在图像上显示检测结果
    if has_red_line:
        print("检测到红线")
    else:
        print("未检测到红线")
    
    # 控制循环频率
    time.sleep(50)  # 50ms延时
    
    if blobs:
        # 找到最大的红色线段
        largest_blob = max(blobs, key=lambda b: b.pixels())
        
        # 绘制识别结果
        img.draw_rectangle(largest_blob.rect(), color=(0, 255, 0))
        img.draw_cross(largest_blob.cx(), largest_blob.cy(), color=(255, 0, 0))
        
        # 计算偏移量
        line_center_x = largest_blob.cx()
        offset = line_center_x - CENTER_X
        offset_percent = int((offset / CENTER_X) * 100)
        
        # 限制偏移量范围
        if offset_percent > 100:
            offset_percent = 100
        elif offset_percent < -100:
            offset_percent = -100
        
        # 显示信息
        img.draw_string(10, 10, "Line Found", color=(255, 255, 255))
        img.draw_string(10, 30, "Offset: %d%%" % offset_percent, color=(255, 255, 255))
        img.draw_string(10, 50, "Center: %d" % line_center_x, color=(255, 255, 255))
        
        return True, offset_percent
    else:
        img.draw_string(10, 10, "No Line Found", color=(255, 0, 0))
        return False, 0

def detect_cross_road():
    """检测十字路口（红线形成十字或T字形状）"""
    img = sensor.snapshot()
    
    # 检测整个十字路口区域的红色
    blobs = img.find_blobs([red_threshold], 
                          roi=CROSS_ROI,
                          pixels_threshold=200,   # 十字路口面积较大
                          area_threshold=200,
                          merge=True)
    
    if blobs:
        largest_blob = max(blobs, key=lambda b: b.pixels())
        
        # 判断是否为十字路口
        # 条件：面积大于一定值，且宽度和高度都比较大
        blob_area = largest_blob.pixels()
        blob_width = largest_blob.w()
        blob_height = largest_blob.h()
        
        # 十字路口特征：面积大，宽度和高度都较大
        if blob_area > 800 and blob_width > 80 and blob_height > 60:
            img.draw_rectangle(largest_blob.rect(), color=(255, 255, 0))
            img.draw_string(10, 70, "CROSS DETECTED!", color=(255, 255, 0))
            return True
    
    return False

def send_command(cmd_type, value=0):
    """发送命令给STM32"""
    # 简化的命令格式: "CMD,value\n"
    data = "%s,%d\n" % (cmd_type, value)
    uart.write(data)
    print("Send: " + data.strip())

# 主循环
while True:
    clock.tick()
    
    # 首先检测十字路口
    if detect_cross_road():
        send_command("STOP", 0)  # 发送停止命令
        time.sleep_ms(100)
    else:
        # 正常循迹
        line_found, offset = detect_red_line()
        if line_found:
            send_command("LINE", offset)  # 发送循迹偏移量
        else:
            send_command("LOST", 0)      # 发送丢失信号
    
    # 绘制ROI区域边框
    img = sensor.get_fb()
    img.draw_rectangle(LINE_ROI, color=(255, 255, 0))   # 黄色框 - 循迹区域
    img.draw_rectangle(CROSS_ROI, color=(255, 0, 255))  # 紫色框 - 十字路口检测区域
    
    # 绘制中心参考线
    img.draw_line(CENTER_X, 0, CENTER_X, 240, color=(255, 255, 255))
    
    # 显示帧率
    fps = clock.fps()
    img.draw_string(200, 200, "FPS: %.1f" % fps, color=(255, 255, 255))
    
    print("FPS: %.2f" % fps)
    
    time.sleep_ms(50)  # 适当延时
